package edu.mapi.ir.controllers.reactive;

import edu.mapi.ir.ciberIF.ciberIF;
/**
 * This controller avoids obstacles at a certain distance (taken from jClient)
 * @author ZP
 */
public class AvoidObstacles implements ReactiveController {

	private double turnDistance = 4.0;
	private double avoidDistance = 1.0;
	private double speed = 0.1;
	
	public boolean decide(ciberIF cif) {
		double irSensor0 = 0, irSensor1 = 0, irSensor2 = 0;
				
		if(cif.IsObstacleReady(0))
			irSensor0 = cif.GetObstacleSensor(0);
		if(cif.IsObstacleReady(1))
			irSensor1 = cif.GetObstacleSensor(1);
		if(cif.IsObstacleReady(2))
			irSensor2 = cif.GetObstacleSensor(2);

		if(irSensor0 > turnDistance || irSensor1 > turnDistance ||  irSensor2 > turnDistance) {
			cif.DriveMotors(speed,-speed);
			return true;
		}
		else if(irSensor1>avoidDistance) {
			cif.DriveMotors(speed,0.0);
			return true;
		}
		else if(irSensor2>avoidDistance) {
			cif.DriveMotors(0.0,speed);
			return true;
		}
		return false;
	}

	public String getDescription() {
		return "Avoid Obstacles";
	}

}
